
// import oimo.dynamics.TimeStep;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.Vec3;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;

import { Joint } from "./Joint";
import { SpringDamper } from "./SpringDamper";
import { RotationalLimitMotor } from "./RotationalLimitMotor";
import { Vec3 } from "../../../common/Vec3";

import { M } from "../../../common/M";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { TimeStep } from "../../TimeStep";
import { Mat3 } from "../../../common/Mat3";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";
import { UniversalJointConfig } from "./UniversalJointConfig";
import { JointType } from "./JointType";

/**
 * A universal joint constrains two rigid bodies' constraint axes to be perpendicular
 * to each other. Rigid bodies can rotate along their constraint axes, but cannot along
 * the direction perpendicular to two constraint axes. This joint provides two degrees
 * of freedom. You can enable lower and upper limits, motors, spring and damper effects
 * of the two rotational constraints.
 */
export class UniversalJoint extends Joint {
	_sd1: SpringDamper;
	_sd2: SpringDamper;
	_lm1: RotationalLimitMotor;
	_lm2: RotationalLimitMotor;

	_axisX: Vec3;
	_axisY: Vec3;
	_axisZ: Vec3;

	_angleX: number;
	_angleY: number;
	_angleZ: number;

	xSingular: boolean;
	ySingular: boolean;
	zSingular: boolean;

	linearError: Vec3;

	/**
	 * Creates a new universal joint by configuration `config`.
	 */
	constructor(config: UniversalJointConfig) {
		super(config, JointType.UNIVERSAL);

		M.vec3_fromVec3(this._localBasisX1, config.localAxis1);
		M.vec3_fromVec3(this._localBasisZ2, config.localAxis2);
		this.buildLocalBasesFromX1Z2();

		this._angleX = 0;
		this._angleY = 0;
		this._angleZ = 0;

		this.xSingular = false;
		this.ySingular = false;
		this.zSingular = false;

		this._sd1 = config.springDamper1.clone();
		this._sd2 = config.springDamper2.clone();
		this._lm1 = config.limitMotor1.clone();
		this._lm2 = config.limitMotor2.clone();
	}


	private updateConstraintAxes(): void {
		var rot1: Mat3 = new Mat3();
		var rot2: Mat3 = new Mat3();
		M.mat3_fromCols(rot1, this._basisX1, this._basisY1, this._basisZ1);
		M.mat3_fromCols(rot2, this._basisX2, this._basisY2, this._basisZ2);

		//     local --(rot1)--> body1
		//     local --(rot2)--> body2
		//     body1 --(relRot)--> body2
		// and
		//     body1 -------------(relRot)------------> body2
		//     body1 --(inv(rot1))--> local --(rot2)--> body2
		//
		// so relative rotation matrix is
		//     inv(rot1) * rot2
		// and NOT
		//     rot2 * inv(rot1)
		var relRot: Mat3 = new Mat3();
		M.mat3_mulLhsTransposed(relRot, rot1, rot2);

		var angleAxisX: Vec3 = new Vec3();
		var angleAxisY: Vec3 = new Vec3();
		var angleAxisZ: Vec3 = new Vec3();
		M.vec3_assign(angleAxisX, this._basisX1);
		M.vec3_assign(angleAxisZ, this._basisZ2);
		M.vec3_cross(angleAxisY, angleAxisZ, angleAxisX); // right-handed coordinate system

		// constraint axes are not equal to rotation axes of Euler angles, because rotation axes
		// of Euler angles are not orthogonal. if we want to constrain relative angular velocity
		// w2-w1 along X-axis of Euler angles, w2-w1 should fulfill
		//   w2-w1 = alpha * angleAxisY + beta * angleAxisZ
		// so
		//   (w2-w1) dot (angleAxisY cross angleAxisZ) = 0
		//
		// be careful about the fact that this does NOT mean
		//   (w2-w1) dot angleAxisX = 0
		//
		// note that we can directory use Y-axis of Euler angles to constrain relative velocity
		// along the axis, as `angleAxisY` is parallel to `angleAxisX cross angleAxisZ`.
		M.vec3_cross(this._axisX, angleAxisY, angleAxisZ);
		M.vec3_assign(this._axisY, angleAxisY);
		M.vec3_cross(this._axisZ, angleAxisX, angleAxisY);

		M.vec3_normalize(this._axisX, this._axisX);
		M.vec3_normalize(this._axisY, this._axisY);
		M.vec3_normalize(this._axisZ, this._axisZ);

		this.xSingular = M.vec3_dot(this._axisX, this._axisX) == 0;
		this.ySingular = M.vec3_dot(this._axisY, this._axisY) == 0;
		this.zSingular = M.vec3_dot(this._axisZ, this._axisZ) == 0;
	}

	public getInfo(info: JointSolverInfo, timeStep: TimeStep, isPositionPart: boolean): void {
		// compute ERP
		var erp: number = this.getErp(timeStep, isPositionPart);

		// compute rhs
		var linearRhs: Vec3 = new Vec3();
		M.vec3_scale(linearRhs, this.linearError, erp);
		var linRhsX: number = M.vec3_get(linearRhs, 0);
		var linRhsY: number = M.vec3_get(linearRhs, 1);
		var linRhsZ: number = M.vec3_get(linearRhs, 2);
		var angRhsY: number = this._angleY * erp;

		var crossR1: Mat3 = new Mat3();
		var crossR2: Mat3 = new Mat3();
		M.vec3_toCrossMatrix(crossR1, this._relativeAnchor1);
		M.vec3_toCrossMatrix(crossR2, this._relativeAnchor2);
		M.mat3_negate(crossR1, crossR1);
		M.mat3_negate(crossR2, crossR2);

		var row: JointSolverInfoRow;
		var j: JacobianRow;
		var motorMassX: number = (this.computeEffectiveInertiaMoment(this._axisX));
		var motorMassZ: number = (this.computeEffectiveInertiaMoment(this._axisZ));

		// linear X
		row = info.addRow(this._impulses[0]);
		row.equalLimit(linRhsX, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 1, 0, 0);
		M.vec3_set(j.lin2, 1, 0, 0);
		M.mat3_getRow(j.ang1, crossR1, 0);
		M.mat3_getRow(j.ang2, crossR2, 0);

		// linear Y
		row = info.addRow(this._impulses[1]);
		row.equalLimit(linRhsY, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 1, 0);
		M.vec3_set(j.lin2, 0, 1, 0);
		M.mat3_getRow(j.ang1, crossR1, 1);
		M.mat3_getRow(j.ang2, crossR2, 1);

		// linear Z
		row = info.addRow(this._impulses[2]);
		row.equalLimit(linRhsZ, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 0, 1);
		M.vec3_set(j.lin2, 0, 0, 1);
		M.mat3_getRow(j.ang1, crossR1, 2);
		M.mat3_getRow(j.ang2, crossR2, 2);

		// angular X
		if (!this.xSingular && (this._sd1.frequency <= 0 || !isPositionPart)) {
			row = info.addRow(this._impulses[3]);
			this.setSolverInfoRowAngular(row, this._angleX, this._lm1, motorMassX, this._sd1, timeStep, isPositionPart);

			j = row.jacobian;
			M.vec3_assign(j.ang1, this._axisX);
			M.vec3_assign(j.ang2, this._axisX);
		}

		// angular Y
		if (!this.ySingular) {
			row = info.addRow(this._impulses[4]);
			row.equalLimit(angRhsY, 0);

			j = row.jacobian;
			M.vec3_assign(j.ang1, this._axisY);
			M.vec3_assign(j.ang2, this._axisY);
		}

		// angular Z
		if (!this.zSingular && (this._sd2.frequency <= 0 || !isPositionPart)) {
			row = info.addRow(this._impulses[5]);
			this.setSolverInfoRowAngular(row, this._angleZ, this._lm2, motorMassZ, this._sd2, timeStep, isPositionPart);

			j = row.jacobian;
			M.vec3_assign(j.ang1, this._axisZ);
			M.vec3_assign(j.ang2, this._axisZ);
		}
	}


	private computeErrors(): void {
		var rot1: Mat3 = new Mat3();
		var rot2: Mat3 = new Mat3();
		M.mat3_fromCols(rot1, this._basisX1, this._basisY1, this._basisZ1);
		M.mat3_fromCols(rot2, this._basisX2, this._basisY2, this._basisZ2);

		//     local --(rot1)--> body1
		//     local --(rot2)--> body2
		//     body1 --(relRot)--> body2
		// and
		//     body1 -------------(relRot)------------> body2
		//     body1 --(inv(rot1))--> local --(rot2)--> body2
		//
		// so relative rotation matrix is
		//     inv(rot1) * rot2
		// but NOT
		//     rot2 * inv(rot1)
		var relRot: Mat3 = new Mat3();
		M.mat3_mulLhsTransposed(relRot, rot1, rot2);

		var angles: Vec3 = new Vec3();
		M.mat3_toEulerXyz(angles, relRot);
		this._angleX = M.vec3_get(angles, 0);
		this._angleY = M.vec3_get(angles, 1);
		this._angleZ = M.vec3_get(angles, 2);

		// compute linear error
		M.vec3_sub(this.linearError, this._anchor2, this._anchor1);
	}

	// --- internal ---

	public _syncAnchors(): void {
		super._syncAnchors();
		this.updateConstraintAxes();

		// compute positional errors
		this.computeErrors();
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		super._getVelocitySolverInfo(timeStep, info);
		this.getInfo(info, timeStep, false);
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		super._getPositionSolverInfo(info);
		this.getInfo(info, null, true);
	}

	// --- public ---

	/**
	 * Returns the first rigid body's constraint axis in world coordinates.
	 */
	public getAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis in world coordinates.
	 */
	public getAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisZ2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisZ2);
	}

	/**
	 * Returns the first rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisZ2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisZ2);
	}

	/**
	 * Returns the rotational spring and damper settings along the first body's constraint axis.
	 */
	public getSpringDamper1(): SpringDamper {
		return this._sd1;
	}

	/**
	 * Returns the rotational spring and damper settings along the second body's constraint axis.
	 */
	public getSpringDamper2(): SpringDamper {
		return this._sd2;
	}

	/**
	 * Returns the rotational limits and motor settings along the first body's constraint axis.
	 */
	public getLimitMotor1(): RotationalLimitMotor {
		return this._lm1;
	}

	/**
	 * Returns the rotational limits and motor settings along the second body's constraint axis.
	 */
	public getLimitMotor2(): RotationalLimitMotor {
		return this._lm2;
	}

	/**
	 * Returns the rotation angle along the first body's constraint axis.
	 */
	public getAngle1(): number {
		return this._angleX;
	}

	/**
	 * Returns the rotation angle along the second body's constraint axis.
	 */
	public getAngle2(): number {
		return this._angleZ;
	}

}
